/*
 * Sun SPOT position calculator using built-in accelerometer
 *
 * Created on Aug 1, 2008 9:47:53 PM;
 */

package sunspot.positioncalculator;

import com.sun.spot.peripheral.Spot;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.IAccelerometer3D;
import com.sun.spot.sensorboard.peripheral.ILightSensor;
import com.sun.spot.sensorboard.peripheral.ISwitch;
import com.sun.spot.sensorboard.peripheral.ISwitchListener;
import com.sun.spot.sensorboard.peripheral.ITemperatureInput;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.peripheral.radio.IRadioPolicyManager;
import com.sun.spot.io.j2me.radiostream.*;
import com.sun.spot.io.j2me.radiogram.*;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.util.*;


import java.io.*;
import javax.microedition.io.*;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 *
 * The startApp method of this class is called by the VM to start the
 * application.
 *
 * @Autor Aakash Polra
 * Last modified: 25 November 2008
 */

public class StartApplication extends MIDlet implements ISwitchListener
{
    
    //NOTE: Change this to your base-station address!
    //Optionally you can change the code to send a broadcast instead!
    public static String HOST_ADDRESS = "0014.4F01.XXXX.XXXX";
    public static String HOST_PORT = "78";
    
    
    //On-board accelerometer instance
    private IAccelerometer3D accelerometer;
    
    //Switch - used to reset velocity/position values
    private ISwitch sw1, sw2;
    
    
    //variables to store positions in different directions (note: only xPos has been utilised at this stage)
    private double xPos, yPos, zPos;
    
    //Instance of PositionCalculator thread
    private PositionCalculator posCalc;
    
    //Instance of data broadcaster thread
    private Broadcaster broadcaster;
    
    
    
    public StartApplication()
    {
        this.xPos = this.yPos = this.zPos = 0.0;
     
        this.accelerometer = EDemoBoard.getInstance().getAccelerometer();
        this.sw1 = EDemoBoard.getInstance().getSwitches()[0];
        this.sw2 = EDemoBoard.getInstance().getSwitches()[1];
        
        //Switch1 is used to reset the position (intended for better testing at this stage)
        this.sw1.addISwitchListener(this);
        
        
        //Position calculator thread calculates position from acceleration values
        this.posCalc = new PositionCalculator();
        
        //Broadcaster broadcasts position values which can be captured remotely (e.g. by a base-station)
        this.broadcaster = new Broadcaster();
    }
    
    
    protected void startApp() throws MIDletStateChangeException {
        posCalc.start();
        broadcaster.start();
        
    }

    
    protected void pauseApp() {
        // This is not currently called by the Squawk VM
    }
    

    /**
     * Called if the MIDlet is terminated by the system.
     * I.e. if startApp throws any exception other than MIDletStateChangeException,
     * if the isolate running the MIDlet is killed with Isolate.exit(), or
     * if VM.stopVM() is called.
     * 
     * It is not called if MIDlet.notifyDestroyed() was called.
     *
     * @param unconditional If true when this method is called, the MIDlet must
     *    cleanup and release all resources. If false the MIDlet may throw
     *    MIDletStateChangeException  to indicate it does not want to be destroyed
     *    at this time.
     */
    protected void destroyApp(boolean unconditional) throws MIDletStateChangeException {
        
        posCalc.stop();
        broadcaster.stop();
    }

    
    //Resetting velocity and position values when switch is pressed
    public void switchPressed(ISwitch sw)
    {
        posCalc.xVel = 0.0;
        xPos = 0.0;
    }

    
    public void switchReleased(ISwitch sw)
    {   //empty def
    }
    
    
    
    private class Broadcaster implements Runnable
    {
        private boolean stopRequested;
        private boolean isRunning;
        private long sleepTime;
        
        public Broadcaster()
        {
            stopRequested = false;
            isRunning = false;
            sleepTime = 300;
        }
        
        
        public synchronized  void start()
        {
            if(!isRunning)
            {
                stopRequested = false;
                new Thread(this).start();
            }
        }
        
        public synchronized  void stop()
        {
            stopRequested = true;
        }
        
        
        public void run()
        {
            isRunning = true;
            
            //
            RadiogramConnection conn = null;
            
            try
            {
                conn = (RadiogramConnection)Connector.open("radiogram://" + HOST_ADDRESS + ":" + HOST_PORT);
                Datagram datagram = conn.newDatagram(conn.getMaximumLength());
                
                
                while(!stopRequested)
                {
                    try
                    {
                        datagram.reset();
                        datagram.writeFloat((float)xPos);
                        //datagram.writeFloat((float)posCalc.xVel);
                        //datagram.writeFloat((float)zPos);
                        
                        conn.send(datagram);
                        
                        Utils.sleep(sleepTime);
                        Thread.yield(); //In case sleep-time is not enough.
                                        //Note this thread is not so important as PositionCalculator thread
                    }
                    catch(Exception expDatagram)
                    {}
                }
            }
            catch(IOException exp)
            {
                System.err.println("Unable to send datagram: " + exp);
            }
            finally
            {
                if(conn != null)
                {
                    try{
                        conn.close();
                    }
                    catch(IOException e)
                    {
                    }
                }
                
                isRunning = false;
            }
            
           /*while(!stopRequested)
            {
            
                //TEMP:TESTING
                System.out.println("Pos=" + xPos);
                //END:TEMP


                Utils.sleep(sleepTime);
                Thread.yield();
            }*/
        }
        
    }
    
    
    
    private class PositionCalculator implements Runnable
    {
        private boolean stopRequested;
        private boolean isRunning;
        private long sleepTime;
        
        
        //Variables to store current velocity values (only xVel has been utilised at this stage)
        private double xVel, yVel, zVel;
        
        
        public PositionCalculator()
        {
            sleepTime = 0;
            stopRequested = false;
            isRunning = false;
            
            xVel = yVel = zVel = 0.0;
            
        }
        
        public synchronized void start()
        {
            if(!isRunning)
            {
                stopRequested = false;
                new Thread(this).start();
            }
        }
        
        public synchronized void stop()
        {
            stopRequested = true;
        }
        
        
        
        public void run()
        {
            isRunning = true;
            
            
            double g = 1.0; //g value (gravitational acceleration). It is used to cancel effects of "g" on e.g. x-axis acceleration when sun-spot is tilted
            double lastAcceleration = 0.0;  //used to take "average" of acceleration value (lastTime + current / 2)
            
            
            //#region Changable Values: 
            double accThreshold = 1.2;  //defines a minimum acceleration value to cancel out the noise
            double velThreshold = 0.1;  //defines a minimum velocity value which is used to cancel-out noise effects and other errors
            
            int aCounter = 0, vCounter = 0; //counters for acceleration and velocity - used to cancel velocity errors
            final int MAX_A_COUNTER = 10, MAX_V_COUNTER = 30;   //Maximum counter-values which if reached, velocity should be reset to 0.
            //#endregion
            
            
            long lastTime = System.currentTimeMillis();
            
            while(!stopRequested)
            {
                
                try
                {
                    //Getting current total acceleration and deciding whether any movement is happenning or not
                    double result = (Math.sin(accelerometer.getTiltX()) - accelerometer.getAccelX()) * 9.8;
                    
                    result = (result + lastAcceleration)/2.0;   //average of current and last acceleration values
                    
                    //Time (t) in seconds
                    double t = (System.currentTimeMillis() - lastTime)/1000.0;
                    lastTime = System.currentTimeMillis();
                    
                    //d- distance
                    double d = 0.0;
                    
                    
                    if(Math.abs(result) > accThreshold)
                    {
                        //Calculate d for changing acceleration
                        double rT = result * t;
                        d = (xVel + (rT/2.0)) * t;    //d = v0t + 0.5at^2

                        //Refresh v0
                        xVel += rT; //v = v0 + at

                        
                        //Resetting acceleration-counter as acceleration was detected
                        aCounter = 0;
                        
                    }
                    else
                    {
                        //Distance for constant velocity (no acceleration)
                        d = xVel * t;
                        
                        //No acceleration detected - incrementing acceleration counter
                        aCounter++;
                    }
                    
                    
                    //=========================================================
                    //Cancelling out Velocity Error!
                    //=========================================================
                    if(Math.abs(xVel) < velThreshold)
                    {
                        vCounter++;
                    }
                    else
                    {
                        vCounter = 0;
                    }
                    
                    
                    //Resetting velocity value if no acceleration was detected and
                    //velocity was below a threshold value
                    if(vCounter > MAX_V_COUNTER && aCounter > MAX_A_COUNTER)
                    {
                        xVel = 0.0;
                        vCounter = 0;
                    }
                    
                    
                    
                    
                    //=========================================================
                    //Updating Position value
                    //=========================================================
                    xPos += d;

                    
                    
                    //Printing comma seperated values so that they can be copied to an excel-like application and
                    //graphs can be drawn. This area can be improved later on. This a quick approach for testing only.
                    //TEMP
                    //Acceleration, Velocity, Time-difference (t), position, d
                    System.out.println(result + "," + xVel + "," + t + "," + xPos + "," + d);
                   //END:TEMP
                    
                }
                catch (IOException ex)
                {
                    ex.printStackTrace();
                }

                
                Utils.sleep(sleepTime);
                Thread.yield();
            }
            
            
            isRunning = false;
            
        }
        
    }
}
